package com.map.astar
{
	import com.map.GridVO;
	import com.map.MapGrid;

	public class AStar
	{
		private var _grid:MapGrid;
		private var _startNode:GridVO;
		private var _endNode:GridVO;
		private var _openList:Array;
		private var _closeList:Array;
		private var _path:Array;
		private static const STRAIGHT_COST:Number = 1;
		private static const DIAG_COST:Number = Math.SQRT2;
		
		public function AStar(grid:MapGrid)
		{
			_grid = grid;
		}
		
		public function findPath(start:GridVO, end:GridVO):Array {
			_openList = [];
			_closeList = [];
			_startNode = start;
			_endNode = end;
			_startNode.g = 0;
			_startNode.h = diagnose(_startNode);
			_startNode.f = _startNode.g + _startNode.h;
			if(search()) {
				return _path;	
			}
			return null;
		}
		
		private function diagnose(curNode:GridVO):Number {	//启发函数：勾股定理
			var x_dis:Number = _endNode.xIndex - curNode.xIndex;
			var z_dis:Number = _endNode.zIndex - curNode.zIndex;
			return Math.sqrt(x_dis * x_dis + z_dis * z_dis);
		}
		
		private function search():Boolean {
			var node:GridVO = _startNode;
			while(node != _endNode) {
				var links:Array = _grid.getLinks(node);
				for each(var test:GridVO in links) {
					var cost:Number = STRAIGHT_COST;
					if(test.xIndex != node.xIndex && test.zIndex != node.zIndex) 
						cost = DIAG_COST;
					var g:Number = node.g + cost;
					var h:Number = diagnose(test);
					var f:Number = g + h;
					if(_openList.indexOf(test) >= 0 || _closeList.indexOf(test) >= 0) {
						if(f < test.f) {
							test.g = g;
							test.h = h;
							test.f = f;
							test.parent = node;
						}
					} else {
						test.g = g;
						test.h = h;
						test.f = f;
						_openList.push(test);
						test.parent = node;
					}
				}
				_closeList.push(node);
				if(_openList.length == 0) {
					trace("not found");
					return false;
				}
				_openList.sortOn("f");
				node = _openList.shift();
			}
			buildPath();
			return true;
		}
		
		private function buildPath():void {
			_path = [];
			var node:GridVO = _endNode;
			_path.unshift(node);
			while(node != _startNode) {
				trace("start build path");
				if(!node.parent) return;
				node = node.parent;
				_path.unshift(node);
			}
			
		}
	}
}